/****************************************************************************
 *
 *   Copyright (C) 2024 MicoAir Development Team. All rights reserved.
 *   Adapted from PX4 Autopilot Project (BSD-3-Clause License)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file intrusive_sorted_list.c
 *
 * Implementation of intrusive sorted list.
 */

#include "px4_platform_common/containers/intrusive_sorted_list.h"
#include <string.h>

/* Helper macros to convert between item pointer and node pointer */
#define ITEM_TO_NODE(list, item) \
	((intrusive_sorted_list_node_t *)((uint8_t *)(item) + (list)->node_offset))

#define NODE_TO_ITEM(list, node) \
	((void *)((uint8_t *)(node) - (list)->node_offset))

void intrusive_sorted_list_init(intrusive_sorted_list_t *list, size_t node_offset,
                                 intrusive_sorted_list_compare_fn compare)
{
	list->head = NULL;
	list->node_offset = node_offset;
	list->compare = compare;
}

void intrusive_sorted_list_add(intrusive_sorted_list_t *list, void *item)
{
	if (item == NULL || list == NULL) {
		return;
	}

	intrusive_sorted_list_node_t *new_node = ITEM_TO_NODE(list, item);
	new_node->sibling = NULL;

	/* List is empty, add as head */
	if (list->head == NULL) {
		list->head = new_node;
		return;
	}

	/* Check if new item should be the new head */
	void *head_item = NODE_TO_ITEM(list, list->head);

	if (list->compare(item, head_item)) {
		new_node->sibling = list->head;
		list->head = new_node;
		return;
	}

	/* Find insertion point */
	intrusive_sorted_list_node_t *node = list->head;

	while (node != NULL && node->sibling != NULL) {
		void *sibling_item = NODE_TO_ITEM(list, node->sibling);

		if (list->compare(item, sibling_item)) {
			/* Insert new_node between node and node->sibling */
			new_node->sibling = node->sibling;
			node->sibling = new_node;
			return;
		}

		node = node->sibling;
	}

	/* Reached the end, add to tail */
	node->sibling = new_node;
}

bool intrusive_sorted_list_remove(intrusive_sorted_list_t *list, void *item)
{
	if (item == NULL || list == NULL || list->head == NULL) {
		return false;
	}

	intrusive_sorted_list_node_t *remove_node = ITEM_TO_NODE(list, item);

	/* Check if removing head */
	if (remove_node == list->head) {
		list->head = list->head->sibling;
		remove_node->sibling = NULL;
		return true;
	}

	/* Search for the node */
	for (intrusive_sorted_list_node_t *node = list->head; node != NULL; node = node->sibling) {
		if (node->sibling == remove_node) {
			/* Found it, remove from list */
			node->sibling = remove_node->sibling;
			remove_node->sibling = NULL;
			return true;
		}
	}

	return false;
}

void *intrusive_sorted_list_head(const intrusive_sorted_list_t *list)
{
	if (list == NULL || list->head == NULL) {
		return NULL;
	}

	return NODE_TO_ITEM(list, list->head);
}

bool intrusive_sorted_list_empty(const intrusive_sorted_list_t *list)
{
	return (list == NULL || list->head == NULL);
}

size_t intrusive_sorted_list_size(const intrusive_sorted_list_t *list)
{
	if (list == NULL) {
		return 0;
	}

	size_t count = 0;

	for (intrusive_sorted_list_node_t *node = list->head; node != NULL; node = node->sibling) {
		count++;
	}

	return count;
}

void intrusive_sorted_list_clear(intrusive_sorted_list_t *list)
{
	if (list == NULL) {
		return;
	}

	/* Just clear the head pointer - we don't free memory in intrusive lists */
	list->head = NULL;
}

void *intrusive_sorted_list_next(const intrusive_sorted_list_t *list, const void *item)
{
	if (list == NULL || item == NULL) {
		return NULL;
	}

	intrusive_sorted_list_node_t *node = ITEM_TO_NODE(list, item);

	if (node->sibling == NULL) {
		return NULL;
	}

	return NODE_TO_ITEM(list, node->sibling);
}

